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Abstract 


Many  tasks  require  the  arm  to  move  from  its  initial  position  to  a  specified  target  position 
without  any  constraints  or  via  a  point  for  a  curved  path  in  case  of  obstacle  avoidance.  In 
this  paper  we  presented  a  formulation  to  plan  the  trajectory  of  human  upper  body.  Having 
obtained  the  desired  path  in  Cartesian  space  using  the  minimum  jerk  theory  and 
represented  each  joint  motion  by  a  B-spline  curve  with  unknown  parameters  (i.e.,  control 
points),  an  optimization  approach  instead  of  inverse  kinematics  is  used  to  predict  control 
points  of  each  joint’s  profile  (a  spline  curve).  It  forms  an  optimization  problem  and  the 
cost  function  includes  four  parts:  (1).  The  discomfort  function  that  evaluates  displacement 
of  each  joint  away  from  its  neutral  position;  (2).  The  consistency  function,  which  is  the 
joint  rate  change  (first  derivative)  and  predicted  overall  trend  from  the  initial  point  to  the 
end  point;  (3).  The  non  smoothness  function  of  the  trajectory,  which  is  the  second 
derivative  of  the  joint  trajectory;  (4).  The  non  continuity  function,  which  is  the  amplitudes 
of  joint  angle  rates  at  the  start  and  end  points  In  order  to  emphasize  smooth  starting  and 
ending  conditions.  This  paper  presents  a  high  redundancy  the  upper  body  modeling  with 
15  degrees  of  freedom  and  optimization  approach  to  predict  the  trajectory  of  human 
upper  body.  It  can  be  expandable  to  apply  the  formulation  to  other  parts  of  the  human 
body.  Illustrative  examples  were  presented  and  an  interface  was  set  up  to  visualize  the 
results. 


Key  Words:  Planning  trajectory,  biomechanics,  minimum  jerk,  B-slines,  joint  space. 
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Introduction 


In  industrial  applications  the  movement  of  the  robot  manipulators  are  planned  in  two 
ways:  The  first  approaeh  requires  the  user  to  explieitly  speeify  a  set  of  eonstraints  (e.g., 
eontinuity  and  smoothness)  on  position,  veloeity,  and  aeceleration  of  the  manipulator’s 
generalized  eoordinates  at  seleeted  loeations  (called  knot  points  or  interpolation  points) 
along  the  trajectory.  The  trajectory  planner  then  seleets  a  parameterized  trajectory  from  a 
class  of  functions  (usually  the  elass  of  polynomial  funetions  of  degree  n  or  less,  for  some 
n),  in  the  time  interval  that  “interpolates”  and  satisfies  the  eonstraints  at  the 

interpolation  points.  In  the  seeond  approach,  the  user  explicitly  specifies  the  path  that  the 
manipulator  must  traverse  by  an  analytieal  function,  such  as  a  straight-line  path  in 
Cartesian  eoordinates,  and  the  trajectory  planner  determines  a  desired  trajeetory  either  in 
joint  eoordinates  or  Cartesian  eoordinates  that  approximates  the  desired  path. 

Predietion  of  human  motions  and  postures  is  partieularly  diffieult  beeause  of  two  main 
reasons:  (i)  the  large  number  of  degrees  of  freedom  that  is  required  to  model  realistic 
motion  and  (ii)  the  inverse  kinematie  solution  (i.e.,  predieting  a  posture)  is  not  as 
straightforward  as  in  the  ease  of  robots,  beeause  while  many  solutions  are  mathematieally 
admissible,  they  do  not  make  sense  and  are  unrealistie!  This  has  been  a  long  standing 
problem  in  human  modeling,  simulation,  and  ergonomics.  Indeed,  traditional  algebraic 
and  geometric  IK  methods  are  diffieult  to  implement  and  yield  an  infinite  number  of 
solutions,  one  of  whieh  must  be  seleeted.  Some  numerical  IK  methods  have  been  used  to 
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solve  low  degree-of-freedom  human  models.  For  human  models,  a  realistie  solution  must 
be  determined,  one  that  resembles  the  aetual  motion. 

By  using  a  quaternion  to  represent  rotations  and  translations,  Taylor  (1979)  proposed  an 
approaeh,  called  bounded  deviation  joint  path.  This  approach  requires  a  motion  planning 
phase  that  selects  enough  knot  points  so  that  the  manipulator  can  be  controlled  by  linear 
interpolation  of  joint  values. 

Significant  research  has  also  been  done  on  collision  free  motion  planning.  For  example, 
in  the  early  1980s,  Lozano-Perez  (1984)  introduced  the  concept  of  a  robot’s  configuration 
space,  in  which  the  robot  is  represented  as  a  point-called  a  configuration-  in  a  parameter 
space  encoding  the  robot’s  DOFs-the  configuration  space.  Path  planning  for  a 
dimensioned  robot  is  thus  “reduced”  to  the  problem  of  planning  a  path  for  a  point  in  a 
space  that  has  as  many  dimensions  as  the  robot  has  DOFs.  Two  popular  approaches  were 
introduced  in  the  1980s:  approximate  cell  decomposition,  where  the  free  space  is 
represented  by  a  collection  of  simple  cells  (Brooks  and  Lozano-Perez,  1983),  and 
potential  field  (Khatib,  1986).  Potential  fields  are  used  in  path  planning  to  create  regions 
with  numeric  values  that  give  an  indication  of  a  measure  of  safety  of  that  region.  But 
none  of  these  approaches  extends  well  to  robots  with  more  than  4  or  5  DOFs,  either  the 
number  of  cells  becomes  too  large  or  the  potential  field  has  local  minima. 

Because  the  common  invariant  features  of  these  movements  were  only  evident  in  the 
extracorporal  coordinates  of  the  hand,  there  is  a  strong  indication  that  planning  takes 
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place  in  terms  of  hand  trajectories  rather  than  joint  rotations.  Flash  and  Hogan  (1985) 
presented  a  mathematieal  model  whieh  was  shown  to  prediet  both  the  qualitative  features 
and  the  quantitative  details  observed  experimentally  in  planar,  multi-joint  arm 
movements.  The  objeetive  funetion  is  the  square  of  the  magnitude  of  jerk  (rate  of  ehange 
of  aceeleration)  of  the  hand  integrated  over  the  entire  movement.  This  is  equivalent  to 
assuming  that  a  major  goal  of  motor  eoordination  is  the  produetion  of  the  smoothest 
possible  movement  of  the  hand. 

The  observation  that  uneonstrained,  unperturbed  arm  movements  are  coordinated  in 
terms  of  hand  motion  shows  that  motor  eontrol  is  organized  in  a  hierarehy  of  inereasing 
levels  of  abstraction  (Hogan  et  al.,  1987).  These  arm  motions  are  organized  as  though  a 
disembodied  hand  eould  be  moved  in  spaee;  the  details  of  how  this  is  aehieved  must  then 
be  supplied  by  a  different  level  in  the  hierarehy. 

Other  models  have  also  been  proposed  and  studied.  The  eomparison  of  Nelson  (1983) 
showed  the  remarkable  similarity  of  movements  predieted  by  the  linear-spring  model  and 
minimum-jerk  model.  Uno  et  al.  (1989)  proposed  a  mathematieal  model,  whieh  is 
formulated  by  defining  an  objeetive  funetion,  square  of  the  rate  of  ehange  of  torque 
integrated  over  the  entire  movement. 

Kawato  et  al.  (1988)  studied  the  problems  of  eoordinates  transformation  from  the  desired 
trajectory  to  the  body  eoordinates  and  motor  command  generation.  They  proposed  an 
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iterative  learning  eontrol  as  an  algorithm  for  simultaneously  solving  these  two  problems. 
This  approaeh  appears  to  be  very  attraetive,  but  it  laeks  eapability  of  generalization. 

Bobrow  (1988)  presented  a  path  planning  teehnique,  whieh  makes  use  of  approximations 
of  an  initial  feasible  trajeetory  in  eonjunction  with  an  iterative,  nonlinear  parameter 
optimization  algorithm  to  produee  time-optimal  motions  for  a  manipulator  with  3  DOF’s 
in  a  workspaee  eontaining  obstaeles.  The  Cartesian  path  of  the  manipulator  was 
represented  with  B-spline  polynomials,  and  the  shape  of  this  path  was  varied  in  a  manner 
that  minimized  the  traversal  time.  Obstaele  avoidanee  eonstraints  were  ineluded  in  the 
problem  through  the  use  of  distanee  funetions.  His  method  did  not  prevent  the  arm  from 
eolliding  with  the  obstaele  at  points  other  than  the  tip. 

A  randomized  planner  was  introdueed  (Barraquand  and  Latombe,  1991),  which  was  able 
to  solve  complex  path-planning  problems  for  many-DOF  robots  by  alternating  “down 
motions”  to  track  the  negated  gradient  of  a  potential  field  and  “random  motions”  to 
escape  local  minima.  Later,  a  probabilistic  roadmap  (PRM)  planner  (Kavraki  et  al,  1996) 
was  developed.  By  sampling  the  configuration  space  by  “local”  paths  (typically  straight 
paths),  a  PRM  can  be  created.  Samples  and  local  paths  are  checked  for  collision  using  a 
fast  collision  checker,  which  avoids  the  prohibitive  computation  of  an  explicit 
representation  of  the  free  space. 

Yun  and  Xi  (1996)  used  genetic  algorithms  for  optimum  motion  planning  in  joint  space 
for  robot,  where  some  inter-knots  were  selected  and  their  parameters  and  the  traveling 
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time  of  each  trajectory  segment  were  coded  and  optimized.  Similarly,  Constantinescu  and 
Croft  (2000)  put  up  with  a  smooth  and  time-optimal  trajectory  planning  which  minimizes 
time  under  path  constraints,  torque  limits  and  torque  rate  limits.  The  variables  of  the 
optimization  are  the  end-effector  pseudo-velocities  at  the  preselected  knot-points  along 
the  path  and  the  slopes  of  the  trajectory  in  the  s-s  phase  plane  at  the  path  end-points, 
where  s  is  the  path  parameter,  e.g.,  the  arc  length.  The  path  itself  is  pre-imposed  as  a 
constraint. 

Quinlan  and  Khatib  (1993)  put  up  with  an  elastic  band  concept.  The  free  space  around  the 
path  was  represented  as  a  series  of  hyperspheres,  called  bubbles.  A  bubble  represents  a 
region  of  configuration  space  that  is  free  of  collision.  Covering  the  path  with  those 
bubbles,  a  channel  of  free  space  was  formed  through  which  the  robot’s  trajectory  could 
be  executed.  Later,  Khatib  et  al.  (1999)  used  elastic  strip  method  for  the  collision- free 
path  modification  behaviors  of  the  robots.  An  elastic  strip  represents  the  workspace 
volume  swept  by  a  robot  along  a  preplanned  trajectory.  This  representation  was 
incrementally  modified  by  external  repulsive  forces  originating  from  obstacles  to 
maintain  a  collision-free  path. 

Barring  particular  overriding  circumstances,  natural  movements-and,  more  markedly, 
hand  movements-tend  to  be  smooth  and  graceful.  One  can  then  postulate  that  this 
characteristic  feature  corresponds  to  a  design  principle,  or,  in  other  words,  that  maximum 
smoothness  is  a  criterion  to  which  the  motor  system  abides  in  the  planning  of  end-point 
movements.  Point-to-point  movements  performed  under  a  wide  variety  of  conditions 


7 


using  a  wide  variety  of  limb  segments  exhibit  the  same  velocity  pattern  (Flash  and 
Hogan,  1985;  Hogan  and  Flash,  1987):  a  smooth,  bell-shaped  time  course,  typically 
symmetrical  (or  nearly  so)  about  the  mid-point  of  the  movement,  starting  from  zero, 
growing  to  a  single  peak  and  declining  again  to  zero.  Many  researchers  have  also 
reported  that  the  velocity  profiles  of  rapid-aimed  movements  have  a  global  asymmetric 
bell-shape,  which  is  invariant  over  a  wide  range  of  movement  sizes  and  speeds,  and 
asymmetry  increased  with  higher  accuracy  demands  (Plamondon,  1995,  Part  I,  Part  II; 
Plamondon,  1998). 

Wolpert  et  al.  (1995)  have  studied  the  effects  of  artificial  visual  feedback  on  planar  two- 
joint  arm  movements  to  distinguish  between  the  two  main  groups  of  human  trajectory 
planning  models-those  specified  in  kinematic  coordinates  and  those  specified  in  dynamic 
coordinates.  Their  results  suggested  that  trajectories  are  planned  in  visually  based 
kinematic  coordinates,  and  the  desired  trajectory  is  straight  in  visual  space,  which  is 
incompatible  with  purely  dynamic-base  models  such  as  the  minimum  torque  change 
model. 

Considerable  research  has  been  done  to  obtain  optimal  robot  path.  Saramago  et  al.  (1998; 
2000;  2002)  have  studied  robot  path  with  considering  dynamic  system,  with  payload 
constraints,  and  in  the  presence  of  moving  obstacles.  Pugazhenthi,  et  al.  (2002)  studied 
the  optimal  trajectory  planning  for  Stewart  platform  based  machine  tools.  Li  and 
Ceglarek  (2002)  presented  another  application  of  optimal  trajectory  planning  for  material 
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handeling  of  compliant  sheet  metal  parts  with  considering  part  permanent  deformation, 
trajectory  smoothness,  and  static  obstacle  avoidance. 

Alexander  (1997)  proposed  the  hypothesis  that  trajectories  are  chosen  to  minimize 
metallic  energy  costs.  Ohta  et  al.  (2003)  presented  a  criterion  minimizing  the  hand 
contact  force  change  and  muscle  force  change  over  the  time  of  movement. 

Existing  approaches  are  applied  in  trajectory  planning  of  manipulators  which  normally 
have  only  2  to  3  DOFs  and  up  to  6  at  most.  On  the  other  hand,  for  the  realistic  motion 
generation,  human  models  normally  have  more  than  10  DOFs.  Moreover,  the  eriteria 
used  for  motion  planning  will  be  quite  different.  For  example,  time  optimum  is  always 
selected  for  the  manipulator  trajectory  planning  in  application.  But  for  human  motion, 
this  is  not  always  important;  instead,  human  tends  to  adopt  the  motion  with  least 
discomfort,  effort  and  most  smoothness.  This  leads  to  a  different  research  area  where 
different  strategies  will  be  used  in  human  motion  planning. 

This  paper  presents  a  methodology  to  predict  and  simulate  the  path  generated  by  humans 
in  a  natural  motion  of  the  torso  and  upper  extremity.  While  this  work  has  been  limited  to 
a  1 5  degree  of  freedom  of  the  upper  body,  the  theory  presented  herein  is  expandable  to 
any  part  of  the  body  that  can  be  represented  as  segmental  links  of  a  kinematic  chain.  The 
work  is  based  on  a  mathematical  postulate  that  allows  for  the  prediction  of  naturalistic 
human  motion  using  an  optimization-based  approach. 
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Human  Modeling 

To  establish  a  systematic  method  for  biomechanicahy  modeling  human  anatomy, 
researchers  have  implemented  conventions  for  representing  segmental  links  and  joints. 
Human  anatomy  can  be  represented  as  a  sequence  of  rigid  bodies  {links)  connected  by 
joints.  Of  course,  this  serial  linkage  could  be  an  arm,  a  leg,  a  finger,  a  wrist,  or  any  other 
functional  mechanism.  Joints  in  the  human  body  vary  in  shape,  function,  and  form.  The 
complexity  offered  by  each  joint  must  also  be  modeled,  to  the  extent  possible,  to  enable  a 
correct  simulation  of  the  motion.  The  degree  by  which  a  model  replicates  the  actual 
physical  model  is  called  the  level  of fidelity. 

Perhaps  the  most  important  element  of  a  joint  is  its  function,  which  may  vary  according 
to  the  joint’s  location  and  physiology.  The  physiology  becomes  important  when  we 
discuss  the  loading  conditions  of  a  joint.  In  terms  of  kinematics,  we  shah  address  the 
function  in  terms  of  the  number  of  degrees  of  freedom  associated  with  its  overall 
movement.  Muscle  action,  ligament,  and  tendon  at  a  joint  are  also  important  and 
contribute  to  the  function. 

For  example,  consider  the  elbow  joint,  which  is  considered  a  hinge  or  one  degree-of- 
freedom  (DOF)  rotational  joint  (e.g.,  the  hinge  of  a  door)  because  it  allows  for  flexibility 
and  extension  in  the  sagittal  plane  (Figure  1)  as  the  radius  and  ulna  rotate  about  the 
humerus.  We  shah  represent  this  joint  by  a  cylinder  that  rotates  about  one  axis  and  has 
no  other  motions  (i.e.,  1  DOF).  Therefore,  we  can  now  say  that  the  elbow  is 
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characterized  by  one  DOF  and  is  represented  as  a  cylindrical  rotational  joint  also  shown 
in  Figure  1. 


Figure  1  One  DOF  elbow 


On  the  other  hand,  consider  the  shoulder  eomplex  (Figure  2).  The  glenohumeral  joint 
(shoulder  joint)  is  a  multi-axial  (ball  and  socket)  synovial  joint  between  the  head  of  the 
humerus  (5)  and  the  glenoid  cavity  (6).  There  is  a  4  to  1  incongruency  between  the  large 
round  head  of  the  humerus  and  the  shallow  glenoid  cavity.  A  ring  of  fibrocartilage 
attaches  to  the  margin  of  the  glenoid  eavity  forming  the  glenoid  labrum.  This  serves  to 
form  a  slightly  deeper  glenoid  fossa  for  articulation  with  the  head  of  the  humerus. 


11 


Figure  2  The  shoulder  joint  (1.  Clavicle.  2.  Body  of  scapula.  3.  Surgical  neck  of 
humerus.  4.  Anatomical  neck  of  humerus.  5.  Coracoid  process.  6.  Acromion) 

We  take  into  consideration  the  final  gross  movement  of  the  joint  (Abdel-Malek,  et  al. 
2001;  Yang  et  al,  2003),  as  abduction/adduction  (about  the  anteroposterior  axis  of  the 
shoulder  joint),  fiexion/extension  and  transverse  fiexion/extension  (about  the 
mediolateral  axis  of  the  shoulder  joint).  Note  that  these  motions  provide  for  three 
rotational  degrees  of  freedom  having  their  axis  intersecting  at  one  point.  This  gives  rise 
to  the  effect  of  a  spherical  joint  typically  associated  with  the  shoulder  joint  (Figure  3).  In 
addition,  the  upward/downward  rotation  of  the  scapula  gives  rise  to  two  substantial 
translational  degrees  of  freedom  and  total  5  DOFs  in  the  shoulder  complex.  This  model 
allows  for  consideration  of  the  coupling  between  some  of  the  joints  as  is  the  case  in  the 
shoulder  where  muscles  extend  over  more  than  one  segment.  When  muscles  are  used  to 
lift  the  arm  in  a  rotational  motion,  unwittingly,  a  translational  motion  of  the  shoulder 
occurs. 


12 


Figure  3  Modeling  of  the  shoulder  eomplex  as  three  revolute  and  two  prismatie  DOFs 

The  normal  anatomy  of  the  spine  is  usually  deseribed  by  dividing  up  the  spine  into  3 
major  seetions:  the  cervieal,  the  thoraeie,  and  the  lumbar  spine  (Figure  4).  Below  the 
lumbar  spine  is  a  bone  ealled  the  saerum,  whieh  is  part  of  the  pelvis.  Eaeh  seetion  is 
made  up  of  individual  bones  called  vertebrae.  There  are  7  cervical  vertebrae,  12  thoracic 
vertebrae,  and  5  lumbar  vertebrae. 
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Cervical  ^ 

Spine 

S 

Thoracic  ^ 

Spine  ' 

Lumbar  j 

;  1 

Spina  * 

Vtrtobral  Body  ^ 


Figure  4  Anatomy  of  the  spine 

The  movements  permitted  in  the  vertebral  column  are:  flexion,  extension,  lateral 
movement,  circumduction,  and  rotation.  Flexion,  or  movement  forward,  is  the  most 
extensive  of  all  the  movements  of  the  vertebral  column,  and  is  freest  in  the  lumbar 
region.  Extension,  or  movement  backward,  is  limited  by  the  anterior  longitudinal 
ligament.  It  is  freest  in  the  cervical  region.  The  extent  of  lateral  movement  is  limited  by 
the  resistance  offered  by  the  surrounding  ligaments.  This  movement  may  take  place  in 
any  part  of  the  column,  but  is  freest  in  the  cervical  and  lumbar  regions.  Circumduction  is 
very  limited,  and  is  merely  a  succession  of  the  preceding  movements.  Rotation  is 
produced  by  the  twisting  of  the  intervertebral  fibrocartilages.  This,  although  only  slight 
between  any  two  vertebrae,  allows  of  a  considerable  extent  of  movement  when  it  takes 
place  in  the  whole  length  of  the  column,  the  front  of  the  upper  part  of  the  column  being 
turned  to  one  or  other  side.  This  movement  occurs  to  a  slight  extent  in  the  cervical 
region,  is  freer  in  the  upper  part  of  the  thoracic  region,  and  absent  in  the  lumbar  region. 
Since  the  reach  movement  of  hand  is  not  related  to  the  position  of  the  head,  the  cervical 
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part  of  the  spine  (neek)  is  not  included  in  our  spine  model.  The  other  parts,  thoracic 
region  and  lumbar  region  are  modeled  as  6  DOF  rotations  as  shown  in  Figures  6. 

The  wrist  is  a  collection  of  many  joints  and  bones  with  one  main  purpose,  to  allow 
human  to  use  the  hands.  The  wrist  has  to  be  extremely  mobile.  At  the  same  time,  it  has 
to  provide  the  strength  for  gripping.  The  wrist  (Figure  5)  comprises  eight  separate  small 
bones  called  the  carpal  bones.  These  bones  connect  the  two  bones  of  the  forearm,  the 
radius  and  the  ulna,  to  the  bones  of  the  hand  and  lingers.  The  movements  permitted  in 
the  wrist  joint  are  flexion,  extension,  abduction  and  adduction.  The  wrist-joint  is  a 
condyloid  articulation.  The  parts  forming  it  are  the  lower  end  of  the  radius  and  under 
surface  of  the  articular  disk  above;  and  the  navicular,  lunate,  and  triangular  bones  below. 
The  articular  surface  of  the  radius  and  the  under  surface  of  the  articular  disk  form 
together  a  transversely  elliptical  concave  surface,  the  receiving  cavity.  The  superior 
articular  surfaces  of  the  navicular,  lunate,  and  triangular  form  a  smooth  convex  surface, 
the  condyle,  which  is  received  into  the  concavity.  The  wrist  is  modeled  as  a  joint  with  3 
DOFs  as  shown  in  Figures  6. 


Figure  5  Anatomy  of  the  wrist 
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Figure  6  Modeling  of  the  torso-shoulder- arm 


The  torso,  shoulder  and  arm  are  modeled  using  15  DOFs  in  total  (Figure  6)  as  deseribed 
above.  The  joint  limits  based  on  the  experiments  on  three  human  subjeets  are 
-TT  !  6<q^<7i: !  6 ,  -n  IYl<qj^<n  lYl ,  -n  l\%<q^<n  1 6  ,  -tt  l\'i<q^<  tt  1 6 , 

-7t  l\%<q^<n  1 6  ,  -;r/18  <  <  ;r/6,  -3.81  <  <  3.81,  -3.81  <  <  3.81 , 

-n  ll<q^<n  H,  -2;r/3  <  <  1  l;r/18 ,  -;r/3  <  <  2;r/3  ,  -5;r/6  <  <  0  , 
-;r  <  ^J3  <  0 ,  -TT ll><q^^<7r ll> ,  -tt  19  <q^^<7i;  19  . 


Formulation 

In  order  to  obtain  a  systematie  representation  of  any  serial  kinematie  chain,  we  define 
q  =  [^j  ...  q^^  e  R”  as  the  vector  of  n-generalized  coordinates  defining  the  motion  of 
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a  limb  with  respect  to  another,  where  is  the  individual  DOF  variable.  The  position 
vector  function  (shown  in  Figure  6)  generated  by  a  point  of  interest  written  as  a 
multiplication  of  rotation  matrices  and  position  vectors  is  expressed  by 

Vq)=  :F(q)  =2[n''R,l''P<=®(<l)  (1) 

L^Cq)] 

where  both  'p^  and  'R^  are  defined  using  the  Denavit-Hartenberg  (D-H)  representation 
method  (Denavit  and  Hartenberg  1955)  such  that 

cos^,.  -cosa,sm^.  sina,.  sin^, 
sin^;  cosa,.  cos^,  -sina,.  cos^,. 

0  sin  a,  cos«, 

and  H[a,.  cos^,.  a,,  sin^.  (2) 

where  q.^  is  the  joint  angle  from  x,_j  axis  to  the  x,.  axis,  is  the  shortest  distance 
between  x._j  and  x.  axes,  a,,  is  the  offset  distance  between  z,.  and  z,._j  axes,  and  is 
the  offset  angle  from  z._j  and  z.  axes. 

Since  the  minimum  jerk  model  to  predict  point-to-point  motion  trajectories  is  well 
accepted  and  experimentally  verified  (Flash  and  Hogan,  1985),  We  will  first  adopt  the 
minimum  jerk  mathematical  model  to  get  a  desired  Cartesian  path,  and  then  convert  it  to 
joint  coordinates  with  the  objective  to  address  the  problem  in  joint  space.  Furthermore, 
because  joint  displacements  as  a  function  of  time  are  non-uniform  (free-form)  curves,  we 
will  use  the  concept  of  B-spline  curves  (Pigel  1997)  because  of  their  many  robust 
properties  such  as  differentiability,  local  control  and  convex  hull.  We  will  then 
implement  a  numerical  optimization  algorithm  to  compute  the  control  points 
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characterizing  the  B-spline  eurves,  where  we  will  utilize  Discomfort,  Non  Consistence, 
Non  Smoothness  and  Non  Continuity  as  cost  functions  while  using  distances  to  the 
desired  path  at  seleeted  points  as  a  set  of  eonstraints.  The  end  result  is  an  optimization- 
based  method  using  human  performanee  measures  as  an  effective  method  for  calculating 
joint  path  trajectories  that  look  and  feel  most  natural. 

We  will  use  B-splines  to  represent  joint  displaeements  as  a  funetion  of  time,  one  for  each 
joint.  In  the  following  subseetions,  we  will  first  introduce  basic  concepts  of  B-splines 
followed  by  expressions  of  joint  B-spline  functions  used  in  our  formulation.  The  B-spline 
eurve  of  joint  j  ean  be  obtained  as 

m 

=  ^<t<t^,j  =  l,2,...,n  (3) 

1=0 

where  N.^(t)  is  the  base  funetions,  P/  are  eontrol  points  for  joint  j  and  the  total 
number  of  eontrol  points  for  joint  j  is  m-l-1  (Pigel  1997). 

The  overall  proeedure  is  presented  in  Figure  7,  and  the  path  predietion  module  is  refined 
and  shown  in  Figure  8.  The  input  to  the  algorithm  are  the  start  and  end  points  of  the 
motion,  the  position  of  the  via  point  for  a  curved  path  in  case  of  obstacle  avoidance,  DH 
parameters  of  the  human  model  and  the  time  desired  to  travel  along  the  path.  The 
absolute  time  is  not  very  important  here  and  it  is  the  relative  time  at  that  instant  that 
determines  the  shape  of  the  velocity.  The  planning  in  Cartesian  space  is  to  find  a  3-D 
path  by  minimizing  jerk  (Flash  and  Hogan,  1985).  The  path  then  is  forwarded  to  the 
optimization  module  in  joint  space,  whieh  is  to  find  a  set  of  control  points  for  the  joint  B- 
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splines  that  minimize  discomfort  and  maximize  the  consistence,  smoothness  and 
continuity  of  joint  movements  with  the  hand  moving  along  the  path.  This  module 
actually  does  the  transformation  from  Cartesian  space  to  joint  space. 


End  Points  A  and  B,  Via  Point  C 
DH  parameters  of  human  model 
Travel  Time 


Input 


Path 

>  Prediction 

Module 


Output 


Profile  of  Joint  Motion, 
i.e.,  q{t)  as  B-spine  for 
each  joint 


Figure  7  Path  prediction  illustration 


Parametric 
path  p(t)  in 
Cartesian 


> 


Figure  8  Refined  path  prediction  module 


When  we  have  the  start  and  end  points,  the  posture  prediction  algorithm  (Mi  et  al.  2002) 
is  used  first  to  predict  the  natural  postures  at  the  start  and  end  points.  That  means  one  can 
obtain  and  ,  where  j  =  I,..., n  .  Therefore,  the  optimization  problem  is  defined  as 

Find  P/  ,  i  =  l,...,m-l ,  j  =  l,...,n 
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Minimize 


cost  =  +  wj. 


inconsistency 


nonsmoothness 


+  wj„ 


noncontinuity 


Subject  to 


x(q(0)-p(0  ll=l|x(X^.',3(0P,')  -P(0ll<^ 

/=0 


q,<I^<q) 


(4) 

(5) 

(6) 


where  0<t<t^,  £  is  a  small  positive  number  as  the  tolerance;  P,.  =[^’  •••  P"~\ 

p(0  is  the  path  obtained  from  the  planning  in  Cartesian  space  phase;  Wj ,  Wj ,  and 
are  the  weights  added  to  each  performance  index. 

(1).  The  discomfort  function  of  all  joints: 


fdiscomf  (q)  =  Z  -  ^7^  )  =  Z  ^7  Z  ^^3  W  “  d 

7=1  7=1  V  1=0 


(V) 


where  is  the  neutral  position  of  a  joint  measured  from  the  starting  home 
configuration,  is  a  weight  function  assigned  to  each  joint  for  the  purpose  of  giving 
importance  to  joints  that  are  typically  more  affected  than  others. 

(2).  The  inconsistency  function:  By  comparing  the  two  postures  (initial  and  end  points), 
an  overall  changing  trend  of  each  joint  (increasing  or  decreasing)  can  be  predicted  to 
avoid  the  abrupt  change  of  the  joint  velocity.  As  a  result,  the  consistency  between  the 
joint  rate  change  (first  derivative)  and  predicted  overall  trend  is  evaluated  and  will  be 
added  to  the  cost  function.  The  detailed  formulation  of  this  consistency  is  as  follows 
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trend, 


(8) 


and 


Xo  ^  q°  I 


j  1  if(^;-^;)>o 
[-1  if{qf.-q])<Q 


where 


f. 


inconsistency 


=  X  ( ^^sn{qj  (0)  -  trend  .  + 1)  q^  {t) 


7=1 


sign(qj(t))  = 


ifqj(t)>0 

ifqj(t)<0 


(9) 


(10) 


The  (+1)  in  Eq.  (9)  is  to  make  the  amplitude  of  the  joint  rate  change  still  has  an  effect 
towards  optimizing  a  smooth  joint  trajectory  when  the  first  term  within  the  parenthesis  is 
evaluated  to  be  zero.  The  multiplication  with  the  amplitude  of  this  joint  change  rate  is  to 
enforce  the  underlying  assumption  that  the  smaller  the  joint  angle  change  rate  is,  the 
smoother  the  joint  trajectory  will  be.  It  also  has  significant  effect  on  the  optimization 
process,  by  not  only  qualifying  the  consistency,  but  also  quantifying  it  so  as  to  avoid  the 
zero  gradient  of  this  objective,  which  is  characteristic  of  an  ill-stated  optimization 
problem  statement. 

(3).  The  non  smoothness  function:  The  second  derivative  of  the  joint  trajectory  is 
considered  in  a  non  smoothness  function  as 


f nonsmoothness 

7=1 


(4).  The  non  continuity  function: 


n  n 


f noncontinuity  =z 


7=1  7=1 


(11) 


(12) 
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Once  the  control  points  of  joint  curves  are  selected  by  the  iterative  optimization 
algorithm,  the  cost  function  of  Eq.  (4)  can  be  integrated  (we  integrate  the  first  three  terms 
and  add  the  fourth  term  to  it)  to  obtain  the  total  cost  at  any  point  along  the  path.  The 
same  principle  applies  to  the  distance,  where  the  total  deviation  along  the  path  can  be 
obtained  by  the  integration  of  the  distance  between  the  calculated  and  desired  paths  from 
the  start  to  the  end  points.  In  our  algorithm,  for  simplicity,  the  cost  function  and  distance 
constraints  are  evaluated  by  selecting  representative  points  on  the  path  where  higher 
density  is  distributed  close  to  the  ends  (total  number  of  43  have  been  selected).  Since 
each  joint’s  profile  has  m  +  1  control  points,  the  total  number  of  the  design  variables  will 
be  n{m  + 1)  initially.  In  our  calculation,  the  joint  values  at  the  start  and  end  have  been 
obtained  directly  using  the  posture  prediction  algorithm,  where  we  only  need  to  calculate 
the  remaining  m-1  control  points  for  each  joint,  i.e.,  the  design  variables  for  the 
optimization  are  reduced  to  n{m  - 1) . 


Illustrative  Examples 

Based  on  simulation  experiments,  a  set  of  weights  (50,  100,  1,  1000)  have  been  selected 
for  Wj ,  Wj ,  W3  and  and  modified  feasible  direction  method  has  been  used  for  the 

optimization.  The  overall  calculation  takes  about  17  to  18  seconds  on  a  1.8GHz 
Pentium4  CPU  with  512M  RAM,  which  makes  it  possible  to  be  used  in  real  time  on  a 
higher  speed  end  workstation  with  dual  processors.  An  interface  has  been  implemented  in 
3D  Studio  Max,  which  can  interact  with  user,  call  the  path  prediction  algorithm  to  do 
calculation,  show  results  and  animate  human  motions  in  real  time. 
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(1).  Point-to-point  example: 


Figures  9  to  13  are  snapshots  of  a  predicted  motion,  where  the  digital  human  starts  from 
one  point  and  goes  to  a  target.  The  small  spheres  on  the  path  are  the  constraints  enforced 
on  the  hand  position  when  predicting  the  joint  B-splines.  From  the  time  stamps  of  the 
shown  snapshots,  it  is  easy  to  observe  that  hand  moves  more  slowly  at  the  start  and  end 
than  in  the  middle.  This  is  so-called  bell  shape  velocity  profile,  a  characteristic  of  a 
smooth  and  natural  human  arm  movement  (Flash  and  Flogan,  1985)  and  predictability  of 
this  profile  is  actually  the  strength  of  the  minimum  jerk  model.  The  predicted  joint 
profiles  for  the  15  joints  are  shown  in  Figure  14,  from  which  we  can  see  each  joint  moves 
smoothly  towards  the  final  position. 


Figure  9  Predicted  motion  1  at  time  0 
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Figure  10  Predicted  motion  1  at  time  0.35t^ 


Figure  1 1  Predicted  motion  1  at  time  0.5tf 
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Figure  12  Predicted  motion  1  at  time  0.65t^ 


Figure  13  Predicted  motion  1  at  time 
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Figure  14  Predicted  joint  splines  for  motion  1 


(2).  Curved  and  obstacle  avoidance  example 

For  curved  and  obstacle  avoidance  movements,  it  is  assumed  that  the  hand  is  required,  in 
the  motion  between  the  end  points,  to  pass  through  a  third  specified  point  (for  example, 
an  artificial  intelligence  engine  can  provide  a  via  point  to  pass  so  as  to  go  around  the 
obstacle  by  examining  the  diameter  of  the  obstacle).  So  given  start  and  end  points,  and  a 
third  via  point,  a  curved  path  in  Cartesian  space  can  be  first  generated  (Flash  and  Hogan, 
1985),  while  the  time  passing  through  the  via  point  is  first  solved.  Figures  15  to  19  give 
the  snapshots  of  such  movement  while  the  digital  human  begins  moving  from  an  initial 
posture  with  all  the  joint  angles  at  zero.  The  big  green  sphere  is  the  via  point  and  the 
curve  is  the  Cartesian  path  predicted  using  minimum  jerk  model.  The  small  spheres  are 
where  distance  constraints  are  enforced  during  the  optimization  for  joint  splines.  The 
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straight  line  is  shown  just  for  easy  comparison  with  the  curved  path.  The  joint  profiles 
shown  in  Figure  20  indicate  the  smooth  movement  of  each  joint. 


Figure  1 5  Predicted  motion  2  with  a  via  point  at  time  0 


Figure  16  Predicted  motion  2  with  a  via  point  at  time  0.3t^ 
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Figure  18  Predicted  motion  2  with  a  via  point  at  time  0.7t^ 
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Figure  20  Predicted  joint  splines  for  motion  2  with  a  via  point 


As  shown  from  the  figures,  the  proposed  method  and  algorithm  can  predict  smooth  and 
graceful  movements  of  upper  body  even  for  a  nonlinear  (curved)  path. 
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Conclusions 


The  proposed  method  for  predicting  joint  profiles  is  general  and  is  broadly  applicable  to 
any  type  of  path,  linear  (straight)  or  nonlinear  (curved)  path  trajectories.  Nonlinear  paths 
are  applicable  to  obstacle  avoidance  problems,  where  trajectories  are  deviated  from  the 
typical  linear  point  to  point  motion  with  minimum  jerk.  It  was  shown  that  a 
mathematical  formulation  applicable  to  any  number  of  DOFs  has  been  developed  and 
demonstrated,  were  the  joint  profiles  as  a  function  of  time  are  predicted.  Each  joint 
profile  has  been  defined  by  a  smooth  B-spline,  where  control  points  are  calculated  using 
a  novel  optimization-based  algorithm.  It  was  shown  that  given  any  start  or  end  points,  or 
given  a  via  point  (a  predefined  intermediary  point),  our  algorithm  will  first  check  and 
determine  if  these  points  fall  within  the  reachable  workspace  of  the  digital  human  model. 
Once  deemed  within  reach,  a  Cartesian  path  (including  the  time  to  traverse  through  the 
via  point)  is  first  predicted  based  on  a  minimum  jerk  cost  function  (within  an  iterative 
optimization  algorithm),  followed  by  the  calculation  of  joint  profiles  characterized  by  B- 
splines,  where  the  objective  to  minimize  a  discomfort  function,  non  consistence  function, 
non  smoothness  function,  and  non  continuity  function.  It  was  also  shown  that  the 
experimental  code  associated  with  this  formulation  was  implemented  in  a  graphical  real¬ 
time  simulation  interface.  The  algorithm  is  shown  to  be  robust  and  can  be  extended  to  a 
real  time  environment. 
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